# from threading import enumerate
from importlib.resources import path
import os
from glob import glob
from os.path import join
import numpy as np
import pickle
import json
from pyquaternion import Quaternion

# def get_tofile():
#     dir = os.listdir(show_file)
boxes_type = {
        0: "car",
        1: "bus",
        2: 'Engineering_vehicles',
        3: 'trailer',
        4: 'truck',
        5: 'bicycle',
        6: 'motorcycle',
        7: 'pedestrians',
        8: 'Traffic_cone',
        9: 'barrier'
    }
lines_type = {0: "road_edge",
              1: "dash_line",
              2: "soild_line"}
lines_type_rev = {"road_edge":0,
                  "dash_line":1,
                  "soild_line":2}
def CreateOneBox():
    box = {"psr": {
            "position": {"x": 0, "y": 0, "z": 0},
            "scale": {"x": 0, "y": 0, "z": 0},
            "rotation": {"x": 0.0, "y": 0, "z": 0}},
            "obj_type": "Car",
            "obj_id": '1'}
    return box

def CreateOneline():
    line = {"obj_type": "road_edge",
            "obj_id": '0',
            "points_set":[]}
    return line


class Lines2density():
    # sparsity Lines to dense lines
    # checked by zhihuaLiu
    def __init__(self,pcr,vs):
        self.pcr = pcr
        self.vs = vs

    @staticmethod
    def calc_abc_from_line_2d(x0, y0, x1, y1):
        a = y0 - y1
        b = x1 - x0
        c = x0*y1 - x1*y0
        return a, b, c

    def get_line_cross_point(self, line1, line2):
        # x1y1x2y2
        a0, b0, c0 = self.calc_abc_from_line_2d(*line1)
        a1, b1, c1 = self.calc_abc_from_line_2d(*line2)
        D = a0 * b1 - a1 * b0
        if D == 0:
            return None
        x = (b0 * c1 - b1 * c0) / D
        y = (a1 * c0 - a0 * c1) / D
        return x, y


    def line2density(self, line):
        line_,pcr,vs = [],self.pcr,self.vs
        points_set = line[0]
        for i in range(points_set.shape[0] - 1):
            line1 = points_set[i:i + 2,:2].reshape(-1)
            s_x = (line1[0] - pcr[0])/vs[0]
            s_y = (line1[1] - pcr[1])/vs[1]

            n_x = (line1[0 + 2] - pcr[0])/vs[0]
            n_y = (line1[1 + 2] - pcr[1])/vs[1]

            # x
            if s_x > n_x:
                s_x, s_y, n_x, n_y = n_x, n_y, s_x, s_y
            lins_x = np.arange(int(s_x) + 1,n_x)
            lins_x = lins_x.reshape(-1,1).repeat(2,1)
            lins_x = (lins_x * vs[0] + pcr[0]) 
            lins_y = np.ones_like(lins_x)
            lins_y[:,0] = pcr[1]
            lins_y[:,1] = pcr[1 + 3]
            lins = np.concatenate([lins_x[:,0,np.newaxis],lins_y[:,0,np.newaxis], \
                                    lins_x[:,1,np.newaxis],lins_y[:,1,np.newaxis]],-1)
            for l in lins:
                xy = self.get_line_cross_point(line1,l)
                if xy is not None:
                    line_.append([xy[0],xy[1],0])
            
            # y
            if s_y > n_y:
                s_x, s_y, n_x, n_y = n_x, n_y, s_x, s_y
            lins_y = np.arange(int(s_y) + 1, n_y)
            lins_y = lins_y.reshape(-1,1).repeat(2,1)
            lins_y = (lins_y * vs[1] + pcr[1]) 
            lins_x = np.ones_like(lins_y)
            lins_x[:,0] = pcr[0]
            lins_x[:,1] = pcr[0 + 3]
            lins = np.concatenate([lins_x[:,0,np.newaxis],lins_y[:,0,np.newaxis], \
                                    lins_x[:,1,np.newaxis],lins_y[:,1,np.newaxis]],-1)
            for l in lins:
                xy = self.get_line_cross_point(line1,l)
                if xy is not None:
                    line_.append([xy[0],xy[1],0])
        return (np.array(line_),line[1])

    def go(self,lines):
        line_ = []
        for line in lines:
            line_.append(self.line2density(line))
        return line_

def Toshow(points, boxes, lines, imgs=[], calib=None, pcd_file=None, save_dir=None, show_dir=None, is_bin=True):
    # points: x y z i -> np.array
    # boxes:[(box,type,id),(),()...]
    # lines: [(pts,type,id),(),()...]
    # pose:
    # pcd_file:
    # save_dir:
    # show_dir:
    if save_dir is None:
        raise
    show_path = os.path.join(show_dir, save_dir)
    if not os.path.exists(show_path):
        os.mkdir(show_path)

    if not os.path.exists(os.path.join(show_path, 'lidar')):
        os.mkdir(os.path.join(show_path, 'lidar'))
    
    if not os.path.exists(os.path.join(show_path, 'label')):
        os.mkdir(os.path.join(show_path, 'label'))
    
    if not os.path.exists(os.path.join(show_path, 'calib/camera')):
        os.makedirs (os.path.join(show_path, 'calib/camera'))
        # if not os.path.join(show_path, 'calib/camera/front'):
        #     os.makedirs(os.path.join(show_path, 'calib/camera/front'))
    if not os.path.exists(os.path.join(show_path, 'camera')):
        os.mkdir(os.path.join(show_path, 'camera'))
    if not os.path.exists(os.path.join(show_path, 'camera','front')):
        os.mkdir(os.path.join(show_path, 'camera','front'))
    if not os.path.exists(os.path.join(show_path, 'camera','left')):
        os.mkdir(os.path.join(show_path, 'camera','left'))
    if not os.path.exists(os.path.join(show_path, 'camera','right')):
        os.mkdir(os.path.join(show_path, 'camera','right'))
    if is_bin:
        pcd_save_file = os.path.join(show_path, 'lidar', pcd_file + '.bin')
    else:
        pcd_save_file = os.path.join(show_path, 'lidar', pcd_file + '.pcd')
    jsn_save_file = os.path.join(show_path, 'label', pcd_file + '.json')
    pose_save_file_root = os.path.join(show_path, 'calib/camera')
    img_save_file_root = os.path.join(show_path, 'camera')
    # if pose is not None:
    #     pose.tofile(pose_save_file)
    # save pcl by bin
    if is_bin:
        points.tofile(pcd_save_file)
    else:
        cloud = pcl.PointCloud_PointXYZI(points)
        pcl.save(cloud, pcd_save_file, binary=True)
    # img to file

    filist = ['front','left','right']
    for i in range(len(imgs)):
        if i < 3:
            imgs[i].save(os.path.join(img_save_file_root, filist[i], pcd_file + '.jpg'))

    if calib is not None:
        filist = ['front','left','right']
        for i in range(len(calib)):
            data_json = {}
            data_json["extrinsic"] = calib[i][0].tolist()
            data_json["intrinsic"] = calib[i][1].tolist()
            js = json.dumps(data_json)
            with open(os.path.join(pose_save_file_root, filist[i] + '.json'), 'w') as f:
                f.write(js)
    # boxes to json
    
    data_json = []
    for k, box in enumerate(boxes): 
        tmp = CreateOneBox()
        tmp["psr"]["position"]['x'] = box[0][0]
        tmp["psr"]["position"]['y'] = box[0][1]
        tmp["psr"]["position"]['z'] = box[0][2]
        tmp["psr"]["scale"]['x'] = box[0][3]
        tmp["psr"]["scale"]['y'] = box[0][4]
        tmp["psr"]["scale"]['z'] = box[0][5]
        tmp["psr"]["rotation"]['z'] =box[0][6] # -np.pi/2 - box[0][6] 
        tmp["obj_type"] = box[1]
        tmp["obj_id"] = k
        data_json.append(tmp)
    # lines to json
    for k, line in enumerate(lines): 
        tmp = CreateOneline()
        # line_ = line[0][:,0].copy()
        # line[0][:,0] = -line[0][:,1]
        # line[0][:,1] = line_

        tmp["points_set"] = line[0].tolist()
        tmp["obj_type"] = line[1]
        tmp["obj_id"] = k
        data_json.append(tmp)
    # save to json
    js = json.dumps(data_json)
    with open(jsn_save_file, 'w') as f:
        f.write(js)


def ego2lidar_points(points, l2e_r,l2e_t):
    l2e_t[2] = 0
    return (points - l2e_t.reshape(1,-1)) @ np.linalg.inv(l2e_r).T
if __name__=='__main__':
    pass